#include <stdio.h>
#include <spu_mfcio.h>
#include <libmisc.h>
#include "../commons.h"
#include "../dma_defs.h"
#include "../util_func.h"


volatile dma_info info __attribute__ ((aligned(128)));


float squaredEuclideanDistance(float* v1, float *v2, int world_DIMENSION)
{
	int i;
	float distance = 0;
	for(i = 0; i < world_DIMENSION; i++)
	{
		distance += (v1[i] - v2[i]) * (v1[i] - v2[i]);
	}
	return distance;
}



int getDMAdim(int size, int max_dma_size, int space_dim, unsigned long long id){
	int chunk_size;

	// Checks if one or more DMA is required
	chunk_size = size;
	if(chunk_size > max_dma_size){
#if DEBUG_SPE
		fprintf(OUT,"SPE(0x%lld): Data size is greater than %d bytes. Using %d bytes. Remaining %d bytes\n",id,max_dma_size,max_dma_size,(size-max_dma_size));
#endif	
		chunk_size = max_dma_size;
	}

	if((size % 32)!=0){
#if DEBUG_SPE
		chunk_size = floor(size / (32.0f*space_dim))*32*space_dim;
		fprintf(OUT,"SPE(0x%lld): Data size(%d) not multiple of %d*32(mcm between 16 and sizeof(float)). Getting only %d bytes\n",id,size,space_dim,chunk_size);
#endif	
	}
	return chunk_size;
}


unsigned int reserveDMAtag(unsigned long long id){
		unsigned int tag_id;
		// Reserves a TAG ID
	    if((tag_id = mfc_tag_reserve()) == MFC_TAG_INVALID){
#if DEBUG_SPE
 	    	fprintf(OUT,"SPE(0x%lld): ERROR: unable to reserve a tag.\n",id);
#endif	
  	      	return -1;
   		}
		return tag_id;
}


void waitDMAtag(unsigned int tag_id, unsigned long long id){
#if DEBUG_SPE
	fprintf(OUT,"SPE(0x%lld): Writing TAG Mask for DMA waiting.\n",id);
#endif	
	mfc_write_tag_mask(1<<tag_id);
    mfc_read_tag_status_all();
#if DEBUG_SPE
	fprintf(OUT,"SPE(0x%lld): Waiting DMA end.\n",id);
#endif	
}



int main(unsigned long long id, unsigned long long argp){
	
	int iter_number, l, s;
	unsigned int tag_id;
	int num_land;	
	int num_samp, samples_offset, samples_chunk_size, c_size, c_ptr;
	
	float *v_landmarks;
	float v_samples[DMA_SIZE*2] 		__attribute__ ((aligned(128)));
	float v_winners[DMA_SIZE*2] 		__attribute__ ((aligned(128))); 	// contains first winner landmark positions
	float v_winnerValues[DMA_SIZE*2] 	__attribute__ ((aligned(128)));		// contains first winner distance value
	float v_seconds[DMA_SIZE*2] 		__attribute__ ((aligned(128)));		// contains second winner landmark position
	float v_secondValues[DMA_SIZE*2] 	__attribute__ ((aligned(128))); 	// contains second winner distance value
	
	float *sample;
	float *winner;
	int winnerIndex;
	float winnerDistance;
	float *second;
	int secondIndex;
	float secondDistance;




/*****************************************/
/*  Get DMA info 						 */
/*****************************************/
	
	// Reserves a TAG ID
	if((tag_id = mfc_tag_reserve()) == MFC_TAG_INVALID){
		fprintf(OUT,"SPE(0x%lld): ERROR: unable to reserve a tag.\n",id);
		return 1;
	}
	
    // Issues DMA command
	mfc_get(&info,argp,sizeof(info),tag_id,0,0);

	// Waits for completition
	mfc_write_tag_mask(1<<tag_id);
	mfc_read_tag_status_all();	

#if DEBUG_SPE
	fprintf(OUT,"SPE(0x%lld): DMA info retrieved.\n",id);
#endif	
	


/*****************************************/
/*  Data Allocation						 */
/*****************************************/
	num_land = info.land_size/SPACE_DIM/sizeof(float);	
	num_samp = info.sample_size/SPACE_DIM/sizeof(float);

	v_landmarks		= malloc_align(info.land_size,7);





#if 1 
/*****************************************/
/*  Get Landmarks						 */
/*****************************************/

	// Reserves a TAG ID
   	if((tag_id = mfc_tag_reserve()) == MFC_TAG_INVALID){
        fprintf(OUT,"SPE(0x%lld): ERROR: unable to reserve a tag.\n",id);
        return 1;
    }
	
    // Issues DMA command
#if DEBUG_SPE
	fprintf(OUT,"SPE(0x%lld): Trying to get %d bytes for %d landmarks (%d dim space).\n",id,info.land_size,num_land,SPACE_DIM);
#endif	
	mfc_get(v_landmarks,info.land_addr,info.land_size,tag_id,0,0);
#if DEBUG_SPE
	fprintf(OUT,"SPE(0x%lld): %d bytes data retrieved.\n",id,info.land_size);
#endif	

	// Waits for completition
	mfc_write_tag_mask(1<<tag_id);
	mfc_read_tag_status_all();


	// Prints DMA results
	/*
	for(i=0;i<num_land;i++){
    	fprintf(OUT,"%d: %f\n",i,landmarks[i]);
	}
	*/
#endif
	



/****************************************/
/*	Main Loop							*/
/****************************************/
/** Repeat following for each iteration, driven by mailbox (after computation spe stalls and waits for mailbox message which can be
 *  STOP = end computation
 *  CONTINUE = perform another iteration
 */

#if DEBUG_SPE
		fprintf(OUT,"SPE(0x%lld): Started computation ------------------------------------------------\n",id);
#endif	


#if 1
	
	c_size = -1;
	c_ptr = 0;
	samples_offset = 0;
	samples_chunk_size = -1;
	
	/** Performs the first data retrieval **/
	tag_id = reserveDMAtag(id);
	samples_chunk_size = getDMAdim(info.sample_size,DMA_SIZE,SPACE_DIM,id);
	mfc_get(v_samples+samples_offset*DMA_SIZE,info.sample_addr,samples_chunk_size,tag_id,0,0);
	waitDMAtag(tag_id,id);
	
	/** Initializes variables **/
	sample 			= v_samples;
	winner 			= v_samples;
	winnerIndex 	= 0;
	winnerDistance 	= squaredEuclideanDistance(sample,winner,SPACE_DIM);
	second 			= v_samples + SPACE_DIM;
	secondIndex 	= 1*SPACE_DIM;
	secondDistance 	= squaredEuclideanDistance(sample,second,SPACE_DIM);

	
	iter_number = 1;
	float tmpDist;

	while(info.sample_size > 0){
		
		/** Prepares for dual buffered DMA transfer **/
		samples_chunk_size = getDMAdim(info.sample_size,DMA_SIZE,SPACE_DIM,id);
		(iter_number%2 == 0)? (samples_offset=0) : (samples_offset=1);
		
		/** Issues DMA trasfer of data for next computation **/
		tag_id = reserveDMAtag(id);
	    mfc_get(v_samples+samples_offset*DMA_SIZE,info.sample_addr,samples_chunk_size,tag_id,0,0);
	
		/** search winner function **/
		for(s=0;s<((int)(samples_chunk_size/sizeof(sample)/4));s+=SPACE_DIM)	{
			sample = &v_samples[s];
			for(l=0;l<num_land;l+=SPACE_DIM){
				tmpDist = squaredEuclideanDistance(sample,&v_landmarks[l],SPACE_DIM);
				if(tmpDist<winnerDistance){
					winner = &v_landmarks[l];
					winnerIndex = l;
					winnerDistance = tmpDist;
				}else if(tmpDist<secondDistance){
					second = &v_landmarks[l];
					secondIndex = l;
					secondDistance = tmpDist;
				}
			}
		}

		/** waits for DMA completition **/
		waitDMAtag(tag_id,id);
 	
		/** adjust sample pointer after DMA transfer **/
		c_size = samples_chunk_size;
		c_ptr = info.sample_addr;
	    info.sample_size -= samples_chunk_size;
	    info.sample_addr += samples_chunk_size;
		iter_number++;
	}

#endif	


#if DEBUG_SPE
		fprintf(OUT,"SPE (0x%lld): ____________________________________________________  Ended computation\n",(id));
#endif
	return 0;
}
